// ---- serial debug -------------------

// uncomment #define DEBUG_FLAG  1 to debug or troubleshoot issues
// leave it commented for normal use

#define DEBUG_FLAG  1

#ifdef DEBUG_FLAG
#define debug(x) Serial.print(x)
#define debugln(x) Serial.println(x)
#else
#define debug(x)
#define debugln(x)
#endif

// --- shield pins ---------------------

#define SHIELD_LED_PIN GPIO_NUM_10
#define SHIELD_CAN_RX GPIO_NUM_1
#define SHIELD_CAN_TX GPIO_NUM_0

#include <esp32_can.h>      /* https://github.com/collin80/esp32_can */
//#include "wifi_manager.h"
#include <WiFi.h>
#include <WiFiClient.h>
#include <WiFiAP.h>

const char *ssid = "CAN_WIFI";
const char *password = "20240326";
//how many clients should be able to connect to this ESP32

#define MAX_SRV_CLIENTS 10
WiFiServer server;
WiFiClient client[MAX_SRV_CLIENTS];

#define MESH_ID 5555555

typedef struct esp_frame_t {
  int mesh_id;
  unsigned long can_id;
  uint8_t len;
  uint8_t d[8];
};
esp_frame_t esp_frame;
void setup() {

#ifdef DEBUG_FLAG
  Serial.begin(115200);
#endif

  pinMode(SHIELD_LED_PIN, OUTPUT);

  // flash the LED
  digitalWrite(SHIELD_LED_PIN, HIGH);

  debugln("");
  debugln("------------------------");
  debugln("    MrDIY CAN SHIELD");
  debugln("------------------------");
  debugln(" CAN...............INIT");

  /* setup CAN @500kbps */
  
  CAN0.setCANPins(SHIELD_CAN_TX, SHIELD_CAN_RX);
  if (!CAN0.begin(500000)) {
    debugln(" CAN...............FAIL");
    delay(500);
    ESP.restart();
  }
  debugln(" CAN.................OK");
  CAN0.watchFor();
  //CAN0.watchFor(0x257);
  //CAN0.watchForRange(uint32_t id1, uint32_t id2)

  /* setup wifi */ 
  if (!WiFi.softAP(ssid, password)) {
    log_e("Soft AP creation failed.");
    while(1);
  }


  IPAddress myIP = WiFi.softAPIP();
  debug("AP IP address: ");
  debugln(myIP);
  server.begin(23);
  server.setNoDelay(true);

  debugln("Server started");

  digitalWrite(SHIELD_LED_PIN, LOW);
}


byte i=0;
CAN_FRAME can_message;
// CAN message frame
CAN_FRAME_FD message;

void loop() {


    digitalWrite(SHIELD_LED_PIN, HIGH);
    
    //新客户连接
    if (server.hasClient()){
      for(i = 0; i < MAX_SRV_CLIENTS; i++){
        //find free/disconnected spot
        if (!client[i] || !client[i].connected()){
          if(client[i]) client[i].stop();
          client[i] = server.available();
          if (!client[i]) debugln("available broken");
          debug("New client: ");
          debugln(i); debugln(' ');
          debugln(client[i].remoteIP());
          break;
        }
      }
      if (i >= MAX_SRV_CLIENTS) {
        //no free/disconnected spot so reject
        server.available().stop();
      }
    }

    
  if (CAN0.read(can_message)) 
  {

    #ifdef DEBUG_FLAG
      debugln(" CAN0.read.................OK");
      debug("ID: ");
      Serial.println(can_message.id,HEX);
      debug("Extended: ");
      if(can_message.extended) 
      {
        debugln("Yes");
      } else {
        debugln("No");
      }
      debug("Length: ");
      Serial.println(can_message.length,DEC);
      for(i = 0;i < can_message.length; i++) 
      {
        Serial.print(can_message.data.uint8[i],HEX);
        debug(" ");
      }
      debugln();
      debugln();
    #endif


    /* prepare the ESPNow msg from the CAN msg */
    esp_frame.mesh_id = MESH_ID;
    esp_frame.can_id = can_message.id;
    esp_frame.len = can_message.length;
    for (int i = 0; i < can_message.length; i++) 
    {
      esp_frame.d[i] = can_message.data.byte[i];
    }

    //check clients for data
    for(i = 0; i < MAX_SRV_CLIENTS; i++){

      if (client[i] && client[i].connected()){
        
        client[i].write((uint8_t *)&esp_frame, sizeof(esp_frame));
        
      }
      else {
        
          
        client[i].stop();
        
      }
    }
    
  }


  
    //check clients for data
    for(i = 0; i < MAX_SRV_CLIENTS; i++){

      if (client[i] && client[i].connected()){
        if(client[i].available()){
          //get data from the telnet client and push it to the UART
          while(client[i].available()) Serial.write(client[i].read());
        }
      }
      else {
        
          
        client[i].stop();
        
      }
    }
    
  
   digitalWrite(SHIELD_LED_PIN, LOW);
  
}
